package inference.traffic;

import java.io.File;
import java.util.ArrayList;
import java.util.Collection;
import java.util.Collections;
import java.util.List;
import java.util.Map;
import java.util.Map.Entry;
import java.util.Arrays;
import java.util.NavigableMap;
import java.util.Set;
import java.util.TreeMap;
import java.util.TreeSet;

import cs534.crf.ConditionalRandomField;
import cs534.crf.ConditionalRandomField.Clique;
import cs534.policy.Function2;

import mdp.id.LogicRDDL2DD;

import rddl.EvalException;
import rddl.RDDL;
import rddl.RDDL.PVAR_NAME;
import rddl.State;
import rddl.RDDL.DOMAIN;
import rddl.RDDL.INSTANCE;
import rddl.RDDL.LCONST;
import rddl.RDDL.NONFLUENTS;
import rddl.parser.parser;
import util.Pair;

public class CRFStructure {
	public static final int leg_length = 4;
	public static final int singleton_table_features = 0;
	public static final int singleton_features = singleton_table_features + 1;
	public static final int singleton_active_features = 1;
	public static final int sqrt_pair_features = 4;
	public static final int pair_features = 16;

	private RDDL _rddl = new RDDL();
	
	private State _state;
	private DOMAIN _d;
	private INSTANCE _i;
	private NONFLUENTS _n;
	private TreeMap<PVAR_NAME, ArrayList<ArrayList<LCONST>>> actions_rddl;
	
	// <action-name>__i<x>_<y> -> set of same
	// NOTE: [jhostetler] Don't change the ordering, or you'll break my code.
	private TreeMap< String, Set<String> > adjacency = new TreeMap< String, Set<String> >();
	
	private TreeMap<Pair<Integer, Integer>, Set<Pair<Integer, Integer>>> intersections
		= new TreeMap<Pair<Integer, Integer>, Set<Pair<Integer, Integer>>>();
	
	final int[][] adjacency_list;
	
	public final int Nvertices;
	
	private TreeMap<Pair<Integer, Integer>, Integer> position_to_vertex_map 
		= new TreeMap<Pair<Integer, Integer>, Integer>();
	
	private TreeMap<Integer, Pair<Integer, Integer>> vertex_to_position_map
		= new TreeMap<Integer, Pair<Integer, Integer>>();
	
//	public final ArrayList<Function2<Double, TrafficState, int[]>> features
//		= new ArrayList<Function2<Double, TrafficState, int[]>>();
	
	public CRFStructure( String dom, String inst ){
		
		try {
			
			File f1 = new File(dom);
			File f2 = new File(inst);
			
			_rddl.addOtherRDDL( parser.parse( f1 ) );
			_rddl.addOtherRDDL( parser.parse( f2 ) );
			
			_state = new State();

			_d = _rddl._tmDomainNodes.entrySet().iterator().next().getValue();
			_i = _rddl._tmInstanceNodes.entrySet().iterator().next().getValue();
			_n = _rddl._tmNonFluentNodes.entrySet().iterator().next().getValue();
			
			initializeState();
			
			actions_rddl = collectActionVars();
			
			PVAR_NAME adjacent = new PVAR_NAME("ADJACENT");
			
			Set<Entry<PVAR_NAME, ArrayList<ArrayList<LCONST>>>> entrySet = actions_rddl.entrySet();
			
			for( Map.Entry<PVAR_NAME, ArrayList<ArrayList<LCONST>>> entry : entrySet ){
				
				for( ArrayList<LCONST> lconst : entry.getValue() ){
					
					String thisOne = LogicRDDL2DD.CleanFluentName( entry.getKey().toString() + lconst );

					if( adjacency.get(thisOne) == null ){
						adjacency.put( thisOne,  new TreeSet<String>() );
					}
					
					for( Map.Entry<PVAR_NAME, ArrayList<ArrayList<LCONST>>> entry2 : entrySet ){
						
						for( ArrayList<LCONST> lconst2 : entry.getValue() ){
					
							String otherOne =  LogicRDDL2DD.CleanFluentName( entry2.getKey().toString() + lconst2 );
							
							//domain specific
							LCONST intersection1 = lconst.get(0);
							LCONST intersection2 = lconst2.get(0);
							
							ArrayList<LCONST> pair = new ArrayList<LCONST>();
							pair.add(intersection1);
							pair.add(intersection2);
							
							Boolean val = (Boolean)(_state.getPVariableAssign(adjacent, pair ));
							
							if( val ){
								
								Set<String> list = adjacency.get( thisOne );
								list.add(otherOne);
								adjacency.put( thisOne, list );
								
							}
							
						}
					}
					
					
				}
				
			}
		} catch (Exception e) {
			e.printStackTrace();
		}
		
		// Done with parsing

		for( Map.Entry<String, Set<String> > thing : adjacency.entrySet() ){
			final Pair<Integer, Integer> p = intersectionNameToPosition( thing.getKey() );
			Set<Pair<Integer, Integer>> neighbors = intersections.get( p );
			if( neighbors == null ) {
				neighbors = new TreeSet<Pair<Integer, Integer>>();
				intersections.put( p, neighbors );
			}
			for( final String s : thing.getValue() ) {
				neighbors.add( intersectionNameToPosition( s ) );
			}
			System.out.println( thing );
		}
		
		Nvertices = intersections.size();
		
		int id = 0;
		for( final Pair<Integer, Integer> p : intersections.keySet() ) {
			vertex_to_position_map.put( id, p );
			position_to_vertex_map.put( p, id );
			++id;
		}
		
		adjacency_list = new int[Nvertices][];
		int i = 0;
		for( Map.Entry<Pair<Integer, Integer>, Set<Pair<Integer, Integer>>> e : intersections.entrySet() ) {
			System.out.print( "" + e.getKey() + " -> { " );
			adjacency_list[i] = new int[e.getValue().size()];
			int j = 0;
			for( final Pair<Integer, Integer> neighbor : e.getValue() ) {
				adjacency_list[i][j++] = positionToVertex( neighbor );
				System.out.print( "" + neighbor + ", " );
			}
			System.out.println( " }" );
			++i;
		}
	}
	
	public Pair<Integer, Integer> intersectionNameToPosition( final String name )
	{
		final String[] tokens = name.split( "_" );
		// Format is: <stuff>_<more-stuff>_..._i<x>_<y>
		return new Pair<Integer, Integer>( Integer.parseInt( tokens[tokens.length - 2].substring( 1 ) ),
										   Integer.parseInt( tokens[tokens.length - 1] ) );
	}
	
	public int intersectionNameToVertex( final String name )
	{
		return adjacency.headMap( name ).size();
	}
	
	public int positionToVertex( final Pair<Integer, Integer> position )
	{
		return position_to_vertex_map.get( position );
	}
	
	public Pair<Integer, Integer> vertexToPosition( final int vertex )
	{
		return vertex_to_position_map.get( vertex );
	}
	
	public static int getOffset( final Pair<Integer, Integer> position, final TrafficState x, final int y )
	{
		int result = 0;
		
		TrafficState.LocalState p = x.getLocalStateDescriptions().get( position );
		assert( p != null );
		
//		for( Map.Entry<String, Boolean> e : p._o1.entrySet() ) {
//			if( "light-signal".equals( e.getKey() ) || "flip".equals( e.getKey() ) ) {
//				continue;
//			}
//			else {
//				result = (result << 1) | (e.getValue().booleanValue() ? 1 : 0);
//			}
//		}
		
//		result = (result << 1) | (p._o1.get( "turn" ).booleanValue() ? 1 : 0);
		result = (result << 2) | p.light_signal; // 2 bits
		result = (result << 2) | y; // 2 bits
		assert( result >= 0 );
		assert( result < singleton_table_features );
		return result;
	}
	
//	public static class TrafficObservation
//	{
//		public double[] x;
//		public TrafficObservation( final double[] x )
//		{ this.x = x; }
//	}
	
	public static class NextSignalIndicator implements Function2<Double, TrafficState, int[]>
	{
		public final int next_signal;
		public NextSignalIndicator( final int next_signal )
		{
			this.next_signal = next_signal;
		}
		
		@Override
		public Double apply( final TrafficState s, final int[] a )
		{
			// TODO Auto-generated method stub
			return null;
		}
	}
	
	/**
	 * Given a signal state and an action, returns the state the signal will
	 * be in after taking the action.
	 * @param current
	 * @param action
	 * @return
	 */
	public static int nextSignalState( final int current, final int action )
	{
		return current ^ action;
	}
	
	public static class TrafficVertexClique extends ConditionalRandomField.Clique<TrafficState>
	{
		public final ArrayList<Function2<Double, TrafficState, int[]>> features;
		public final TrafficFeatureFilter filter;
		public final double[] potential;
		public final Pair<Integer, Integer> position;
		
		public TrafficVertexClique( final ArrayList<Function2<Double, TrafficState, int[]>> features,
									final TrafficFeatureFilter filter, final int vertex, final Pair<Integer, Integer> position )
		{
			super( new int[] { vertex } );
			this.features = features;
			this.filter = filter;
			assert( vertices.length == 1 );
			potential = new double[features.size()];
			this.position = position;
		}
		
		public TrafficVertexClique( final ArrayList<Function2<Double, TrafficState, int[]>> features,
									final TrafficFeatureFilter filter, final int vertex, 
									final Pair<Integer, Integer> position, final double[] potential )
		{
			super( new int[] { vertex } );
			this.features = features;
			this.filter = filter;
			assert( vertices.length == 1 );
			this.position = position;
			assert( potential.length == features.size() );
			this.potential = potential;
		}
		
//		private void init()
//		{
//			grid_cells.add( "c" + (position._o1.intValue() - 2) + "_" + position._o2.intValue() );
//			grid_cells.add( "c" + (position._o1.intValue() - 1) + "_" + position._o2.intValue() );
//			grid_cells.add( "c" + position._o1.intValue() + "_" + (position._o2.intValue() - 2) );
//			grid_cells.add( "c" + position._o1.intValue() + "_" + (position._o2.intValue() - 1) );
//			grid_cells.add( "c" + position._o1.intValue() + "_" + position._o2.intValue() );
//			grid_cells.add( "c" + position._o1.intValue() + "_" + (position._o2.intValue() + 1) );
//			grid_cells.add( "c" + position._o1.intValue() + "_" + (position._o2.intValue() + 2) );
//			grid_cells.add( "c" + (position._o1.intValue() + 1) + "_" + position._o2.intValue() );
//			grid_cells.add( "c" + (position._o1.intValue() + 2) + "_" + position._o2.intValue() );
//		}
		
		@Override
		public double logPotential( final TrafficState x, final int[] y )
		{
			double total_potential = 0.0;
//			for( int i : filter.vertexFeatures( x, y[0], vertices[0] ) ) {
//				total_potential += potential[i]*features.get( i ).apply( x, y );
//			}
			// Feature functions expect a full joint action.
			final int[] fake_joint_action = new int[x.getNumIntersections()];
			fake_joint_action[vertices[0]] = y[0];
			for( int i = 0; i < features.size(); ++i ) {
				final double f_i = features.get( i ).apply( x, fake_joint_action );
				if( f_i != 0.0 ) {
//					System.out.println( "Nonzero feature: f_" + i + " = " + f_i );
					total_potential += potential[i] * f_i;
				}
			}
			return total_potential;
		}
		
		@Override
		public double[] parameters()
		{
			return potential;
		}
		
		@Override
		public int numParameters()
		{
			return features.size();
		}

		@Override
		public Clique<TrafficState> update( final double[] parameters )
		{
			assert( parameters.length == potential.length );
			return new TrafficVertexClique( features, filter, vertices[0], position, parameters );
		}
	}
	
	public static class TrafficEdgeClique extends ConditionalRandomField.Clique<TrafficState>
	{
		public final double[] potential;
		
		public TrafficEdgeClique( final int[] vertices )
		{
			super(vertices);
			assert( vertices.length == 2 );
			potential = new double[pair_features];
		}
		
		public TrafficEdgeClique( final int[] vertices, final double[] potential )
		{
			super(vertices);
			assert( vertices.length == 2 );
			this.potential = potential;
		}

		@Override
		public double logPotential( final TrafficState x, final int[] y )
		{
			return potential[y[0]*sqrt_pair_features + y[1]];
		}
		
		@Override
		public double[] parameters()
		{
			return potential;
		}
		
		@Override
		public int numParameters()
		{
			return pair_features;
		}

		@Override
		public Clique<TrafficState> update( final double[] parameters )
		{
			assert( parameters.length == pair_features );
			return new TrafficEdgeClique( Arrays.copyOf( vertices, vertices.length ), parameters );
		}
	}
	
	public static class One implements Function2<Double, TrafficState, int[]>
	{
		public static final One TheOne = new One();
		
		@Override
		public Double apply( final TrafficState p0, final int[] p1 )
		{
			return 1.0;
		}
	}
	
	public static class VertexIndicator implements Function2<Double, TrafficState, int[]>
	{
		private final int value_;
		public VertexIndicator( final int value )
		{
			value_ = value;
		}
		@Override
		public Double apply( final TrafficState s, final int[] a ) 
		{
			return (a[0] == value_ ? 1.0 : 0.0);
		}
	}
	
	public static class EdgeIndicator implements Function2<Double, TrafficState, int[]>
	{
		private final int[] value_;
		private final int v1_;
		private final int v2_;
		
		public EdgeIndicator( final int v1, final int v2, final int[] value )
		{
			value_ = value;
			v1_ = v1;
			v2_ = v2;
		}
		@Override
		public Double apply( final TrafficState s, final int[] a ) 
		{
			return ((a[v1_] == value_[0] && a[v2_] == value_[1]) ? 1.0 : 0.0);
		}
	}
	
	public static String[] makeNeighborStrings( final Pair<Integer, Integer> position, final int direction )
	{
		final String[] strings = new String[leg_length];
		int idx = 0;
		if( direction == 0 ) {
			for( int i = 1; i <= leg_length; ++i ) {
				strings[idx++] = "c" + (position._o1 + i) + "_" + position._o2;
			}
		}
		else if( direction == 1 ) {
			for( int i = 1; i <= leg_length; ++i ) {
				strings[idx++] = "c" + position._o1 + "_" + (position._o2 - i);
			}
		}
		else if( direction == 2 ) {
			for( int i = 1; i <= leg_length; ++i ) {
				strings[idx++] = "c" + (position._o1 - i) + "_" + position._o2;
			}
		}
		else if( direction == 3 ) {
			for( int i = 1; i <= leg_length; ++i ) {
				strings[idx++] = "c" + position._o1 + "_" + (position._o2 + i);
			}
		}
		else {
			throw new AssertionError( "Unexpected direction" );
		}
		assert( idx == leg_length );
		return strings;
	}
	
	public int[] makeNeighborIndices( final int direction )
	{
		final int[] result = new int[leg_length];
		final int total = 4 * leg_length + 1;
		final int base;
		switch( direction ) {
		case 0: // right
			base = total - leg_length;
			break;
		case 1: // up
			base = leg_length;
			break;
		case 2: // left
			base = 0;
			break;
		case 3: // down
			base = total - (2 * leg_length);
			break;
		default:
			base = -1;
			throw new AssertionError( "Unexpected direction" );
		}
		for( int i = 0; i < leg_length; ++i ) {
			result[i] = base + i;
		}
		return result;
	}
	
	public static class FullnessFeature implements Function2<Double, TrafficState, int[]>
	{
		public final Pair<Integer, Integer> position;
		public final int direction;//same as integer representation of action in TrafficState
		public final int[] neighbor_indices;
		
		public FullnessFeature( final CRFStructure structure, final Pair<Integer, Integer> position, final int direction )
		{
			this.position = position;
			this.direction = direction;
			this.neighbor_indices = structure.makeNeighborIndices( direction );
		}
		
		@Override
		public Double apply( final TrafficState s, final int[] a )
		{
			int count = 0;
			final TrafficState.LocalState p = s.getLocalStateDescriptions().get( position );
//			System.out.println( position );
			for( final int i : neighbor_indices ) {
//				System.out.println( c );
				if( p.occupancy[i] ) {
					count += 1;
				}
			}
			return (double) count;
		}
	}
	
	public static int signalState( final Pair<TreeMap<String, Boolean>, TreeMap<String, Boolean>> signal )
	{
		final int h = signal._o1.get( "light-signal" ).booleanValue() ? 1 : 0;
		final int v = signal._o2.get( "light-signal" ).booleanValue() ? 1 : 0;
		return (h<<1) | v;
	}
	
	public static int signalState( final TrafficState.LocalState signal )
	{
		return signal.light_signal;
	}
	
	public static class FullnessXNextSignalFeature implements Function2<Double, TrafficState, int[]>
	{
		public final int vertex;
		public final Pair<Integer, Integer> position;
		public final int direction;
		public final int target_signal_state;
		public final int[] neighbor_indices;
		
		public FullnessXNextSignalFeature( final CRFStructure structure, final int vertex, 
				final Pair<Integer, Integer> position, 
			   final int direction, final int target_signal_state )
		{
			System.out.println( "Feature: v = " + vertex + ", d = " + direction + ", t = " + target_signal_state );
			this.vertex = vertex;
			this.position = position;
			this.direction = direction;
			this.target_signal_state = target_signal_state;
			this.neighbor_indices = structure.makeNeighborIndices( direction );
			System.out.println( "Neighbors = " + Arrays.toString( this.neighbor_indices ) );
		}
		
		@Override
		public Double apply( final TrafficState s, final int[] a )
		{
			final TrafficState.LocalState p = s.getLocalStateDescriptions().get( position );
			return fullnessXnext_signal( p, a[vertex], neighbor_indices, target_signal_state );
		}
	}
	
	public static double fullnessXnext_signal( final TrafficState.LocalState s, final int a, 
											   final int[] neighbor_indices,
											   final int target_signal_state )
	{
		final int next_state = nextSignalState( s.light_signal, a );
//		System.out.println( "next_state = " + next_state + ", target_state = " + target_signal_state );
		if( next_state != target_signal_state ) {
			return 0.0;
		}
		
//		System.out.println( Arrays.toString( p.occupancy ) );
		int count = 0;
//		System.out.println( position );
		for( final int i : neighbor_indices ) {
//			System.out.println( c );
			if( s.occupancy[i] ) {
//				System.out.println( "Occupied " + i );
				count += 1;
			}
		}
		return (double) count;
	}
	
	public static class LocalClassifierFeature implements Function2<Double, TrafficState, int[]>
	{
		public final CRFStructure crf_structure;
		public final Pair<Integer, Integer> position;
		public final int vertex;
		public final NavigableMap<Integer, ArrayList<Double>> local_lr_model;
		
		public LocalClassifierFeature( final CRFStructure crf_structure,
									   final Pair<Integer, Integer> position,
									   final int vertex,
									   final NavigableMap<Integer, ArrayList<Double>> navigableMap )
		{
			this.crf_structure = crf_structure;
			this.position = position;
			this.vertex = vertex;
			this.local_lr_model = navigableMap;
		}
		
		@Override
		public Double apply( final TrafficState s, final int[] a )
		{
			final TrafficState.LocalState p = s.getLocalStateDescriptions().get( position );
			final ArrayList<Double> local_action = LogisticRegression.getLocalActionProbabilities( p, crf_structure, 
					local_lr_model );
			
			return local_action.get( a[vertex] );
		}
	}
	
	public ArrayList<Function2<Double, TrafficState, int[]>> naiveFeatures()
	{
		final ArrayList<Function2<Double, TrafficState, int[]>> features
			= new ArrayList<Function2<Double, TrafficState, int[]>>();
		// Vertex features
		for( int i = 0; i < Nvertices; ++i ) {
			features.addAll( makeVertexFeatures( i ) );
		}
		
		// Edge features
		for( int i = 0; i < Nvertices; ++i ) {
			for( int j = 0; j < adjacency_list[i].length; ++j ) {
				final int v = adjacency_list[i][j];
				if( v >= i ) { // Prevent double-counting
					// Loop over joint actions
					for( int a = 0; a < 4; ++a ) {
						for( int b = 0; b < 4; ++b ) {
							features.add( new EdgeIndicator( i, v, new int[] { a, b } ) );
						}
					}
				}
			}
		}
		return features;
	}
	
	public ArrayList<Function2<Double, TrafficState, int[]>> sophisticatedFeatures(
		final NavigableMap<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double> > > lr_model, 
		final CRFStructure crf_structure )
	{
		final ArrayList<Function2<Double, TrafficState, int[]>> features
			= new ArrayList<Function2<Double, TrafficState, int[]>>();
		// Vertex features
		for( final Map.Entry<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double> > > e : 
				lr_model.entrySet() ) {
			features.addAll( makeVertexLRFeatures( e.getKey(), positionToVertex( e.getKey() ), e.getValue() ,
					crf_structure ) );
		}
		
		// Edge features
		for( int i = 0; i < Nvertices; ++i ) {
			for( int j = 0; j < adjacency_list[i].length; ++j ) {
				final int v = adjacency_list[i][j];
				if( v >= i ) { // Prevent double-counting
					// Loop over joint actions
					for( int a = 0; a < 4; ++a ) {
						for( int b = 0; b < 4; ++b ) {
							features.add( new EdgeIndicator( i, v, new int[] { a, b } ) );
						}
					}
				}
			}
		}
		return features;
	}
	
	public static NavigableMap<Pair<Integer, Integer>, Pair<Boolean, Boolean>> 
		flatActionToMap( final CRFStructure structure, final int[] action )
	{
		final NavigableMap<Pair<Integer, Integer>, Pair<Boolean, Boolean>> result
			= new TreeMap<Pair<Integer, Integer>, Pair<Boolean, Boolean>>();
		for( int v = 0; v < action.length; ++v ) {
			final Pair<Integer, Integer> position = structure.vertexToPosition( v );
			result.put( position, new Pair<Boolean, Boolean>( (action[v] & (1<<1)) != 0, (action[v] & 1) != 0 ) );
		}
		return result;
	}
	
	public static class TrafficFeatureFilter implements Function2<int[], TrafficState, int[]>
	{
		public final CRFStructure structure_;
		
		public final int num_pairs;
		
		public final NavigableMap<Pair<Integer, Integer>, Integer> pair_clique_map = new TreeMap<Pair<Integer, Integer>, Integer>();

		public TrafficFeatureFilter( final CRFStructure structure )
		{
			structure_ = structure;
			int pairs = 0;
			for( int i = 0; i < structure_.adjacency_list.length; ++i ) {
				for( int j = 0; j < structure_.adjacency_list[i].length; ++j ) {
					if( structure_.adjacency_list[i][j] >= i ) {
						pair_clique_map.put( new Pair<Integer, Integer>( i, structure_.adjacency_list[i][j] ), 
											 beginPairCliques() + (pairs * pair_features) );
						pairs += 1;
					}
				}
			}
			num_pairs = pairs;
		}
		
		public int singletonCliqueIndex( final int v )
		{
			return  v * singleton_features;
		}
		
		public int beginPairCliques()
		{
			return structure_.Nvertices * singleton_features;
		}
		
		public int pairCliqueIndex( final int u, final int v )
		{
			return pair_clique_map.get( new Pair<Integer, Integer>( u, v ) );
		}
		
		public int[] vertexFeatures( final TrafficState s, final int a, final int v )
		{
			final int[] result = new int[singleton_active_features];
			final int clique_idx = getOffset( structure_.vertexToPosition( v ), s, a );
//			System.out.print( " " + clique_idx );
			int idx = 0;
			result[idx++] = clique_idx;
			for( int j = singleton_table_features; j < singleton_features; ++j ) {
				result[idx++] = j;
//				System.out.print( " " + j );
			}
			assert( idx == singleton_active_features );
			return result;
		}
		
		public int[] filterFeatures( final TrafficState s, final int[] a )
		{
			System.out.println( "Action: " + Arrays.toString( a ) );
			System.out.println( "Relevant cliques:" );
			final int[] result = new int[(structure_.Nvertices * singleton_active_features) + num_pairs];
			int idx = 0;
			// Singleton cliques
			System.out.println( "Singletons" );
			for( int i = 0; i < a.length; ++i ) {
				System.out.println( "Vertex " + i );
				final int base = i * singleton_features;
				for( int vi : vertexFeatures( s, a[i], i ) ) {
					System.out.print( " " + vi );
					result[idx++] = base + vi;
				}
				System.out.println();
			}
			// Pair cliques
			System.out.println( "\nPairs" );
			for( final Map.Entry<Pair<Integer, Integer>, Integer> e : pair_clique_map.entrySet() ) {
				final int u = e.getKey()._o1;
				final int v = e.getKey()._o2;
				final int clique_idx = pairCliqueIndex( u, v ) + (sqrt_pair_features * a[u] + a[v]);
				System.out.print( " " + clique_idx );
				result[idx++] = clique_idx;
			}
			System.out.println( "\n" + idx );
			assert( idx == ((structure_.Nvertices * singleton_active_features) + num_pairs) );
			return result;
		}

		@Override
		public int[] apply(TrafficState p0, int[] p1) {
			return filterFeatures( p0, p1 );
		}
	}
	
	@SuppressWarnings("unchecked")
	public static Function2<Double, TrafficState, int[]>[] makeFeatures( final CRFStructure structure, 
																		 final ConditionalRandomField<TrafficState> crf )
	{
		final ArrayList<Function2<Double, TrafficState, int[]>> features 
			= new ArrayList<Function2<Double, TrafficState, int[]>>( crf.numParameters() );
		for( final ConditionalRandomField.Clique<TrafficState> clique : crf.cliques() ) {
			if( clique instanceof TrafficVertexClique ) {
				for( int i = 0; i < singleton_table_features; ++i ) {
					// We're only using indicator features, so the filterFeatures()
					// method takes care of deciding which ones to use.
					features.add( One.TheOne );
				}
				for( int direction = 0; direction < 4; ++direction ) {
					features.add( new FullnessFeature( structure, structure.vertexToPosition( clique.vertices[0] ), direction ) );
				}
			}
			else if( clique instanceof TrafficEdgeClique ) {
				final TrafficEdgeClique tc = (TrafficEdgeClique) clique;
				for( int a = 0; a < 4; ++a ) {
					for( int b = 0; b < 4; ++b ) {
						features.add( new EdgeIndicator( tc.vertices[0], tc.vertices[1], new int[] { a, b } ) );
					}
				}
			}
			else {
				throw new AssertionError( "Unexpected clique type" );
			}
		}
		final Function2<Double, TrafficState, int[]>[] array_type = new Function2[0];
		return (Function2<Double, TrafficState, int[]>[]) features.toArray( array_type );
	}
	
	public ArrayList<Function2<Double, TrafficState, int[]>> makeVertexFeatures( final int v )
	{
		final ArrayList<Function2<Double, TrafficState, int[]>> result 
			= new ArrayList<Function2<Double, TrafficState, int[]>>( 16 );
		for( int direction = 0; direction < 4; ++direction ) {
			for( int signal_state = 0; signal_state < 4; ++signal_state ) {
				result.add( new FullnessXNextSignalFeature( this, v, vertexToPosition( v ), direction, signal_state ) );
			}
		}
		return result;
	}
	
	public ArrayList<Function2<Double, TrafficState, int[]>> 
		makeVertexLRFeatures( final Pair<Integer, Integer> position, final int v, 
							  final NavigableMap<Integer, ArrayList<Double>> local_lr_model, final CRFStructure crf_structure )
	{
		final ArrayList<Function2<Double, TrafficState, int[]>> result
			= new ArrayList<Function2<Double, TrafficState, int[]>>( 1 );
		result.add( new LocalClassifierFeature( crf_structure, position, v, local_lr_model ) );
		return result;
	}
	
	public NavigableMap<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double> > >
		makeNullLRModel( final TrafficState s )
	{
		final NavigableMap<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double> > >
			result = new TreeMap<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double> > >();
		for( final Pair<Integer, Integer> i : intersections.navigableKeySet() ) {
			
			final int num_params = 17;//WARNING : hard coded value here
			// s.getLocalStateDescriptions().get( i ).occupancy.length + 2 + 1;
			
			final ArrayList<Double> zeroes = new ArrayList<Double>( num_params );
			for( int j = 0; j < num_params; ++j ) {
				zeroes.add( 0.0 );
			}
			
			TreeMap<Integer, ArrayList<Double>> inner = new TreeMap< Integer, ArrayList<Double> >();
			
			for( int k = 0 ; k < 4; ++k ){
				inner.put(k, new ArrayList<Double> ( zeroes ) );
			}
			
			result.put( i, inner );
			
		}
		return result;
	}
	
	@SuppressWarnings("unchecked")
	public ConditionalRandomField<TrafficState> createNaiveCrf()
	{
		final int Nvertices = intersections.size();
		final int[] cardinality = new int[Nvertices];
		Arrays.fill( cardinality, 4 );
		
		final List<ConditionalRandomField.Clique<TrafficState>> cliques
			= new ArrayList<ConditionalRandomField.Clique<TrafficState>>();
		
		final TrafficFeatureFilter filter = new TrafficFeatureFilter( this );
		// Vertex cliques
		for( int i = 0; i < Nvertices; ++i ) {
			cliques.add( new TrafficVertexClique( makeVertexFeatures( i ), filter, i, vertexToPosition( i ) ) );
		}
		
		// Edge cliques
		for( int i = 0; i < Nvertices; ++i ) {
			for( int j = 0; j < adjacency_list[i].length; ++j ) {
				final int v = adjacency_list[i][j];
				if( v >= i ) {
					// Prevent double-counting
					System.out.println( "EdgeClique( " + vertexToPosition( i ) + ", " + vertexToPosition( v ) + " )" );
					cliques.add( new TrafficEdgeClique( new int[] { i, v } ) );
				}
			}
		}
		
		final ConditionalRandomField.Clique<TrafficState>[] array_type = new ConditionalRandomField.Clique[0];
		return new ConditionalRandomField<TrafficState>( 
			cardinality, adjacency_list, (ConditionalRandomField.Clique<TrafficState>[]) cliques.toArray( array_type ) );
	}
	
	@SuppressWarnings("unchecked")
	public ConditionalRandomField<TrafficState> createSophisticatedCrf( 
		final NavigableMap<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double>>> lr_model,
			final CRFStructure crf_structure )
	{
		final int Nvertices = intersections.size();
		final int[] cardinality = new int[Nvertices];
		Arrays.fill( cardinality, 4 );
		
		final List<ConditionalRandomField.Clique<TrafficState>> cliques
			= new ArrayList<ConditionalRandomField.Clique<TrafficState>>();
		
		final TrafficFeatureFilter filter = null;
		// Vertex cliques
		int idx = 0;
		for( final Entry<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double>>> e : 
			lr_model.entrySet() ) {
			cliques.add( new TrafficVertexClique( makeVertexLRFeatures( e.getKey(), idx, e.getValue()
					, crf_structure ), 
												  filter, idx, e.getKey() ) );
			++idx;
		}
		
		// Edge cliques
		for( int i = 0; i < Nvertices; ++i ) {
			for( int j = 0; j < adjacency_list[i].length; ++j ) {
				final int v = adjacency_list[i][j];
				if( v >= i ) {
					// Prevent double-counting
					System.out.println( "EdgeClique( " + vertexToPosition( i ) + ", " + vertexToPosition( v ) + " )" );
					cliques.add( new TrafficEdgeClique( new int[] { i, v } ) );
				}
			}
		}
		
		final ConditionalRandomField.Clique<TrafficState>[] array_type = new ConditionalRandomField.Clique[0];
		return new ConditionalRandomField<TrafficState>( 
			cardinality, adjacency_list, (ConditionalRandomField.Clique<TrafficState>[]) cliques.toArray( array_type ) );
	}
	
	@SuppressWarnings("unchecked")
	public ConditionalRandomField<TrafficState> updateSophisticatedCrf( 
		final ConditionalRandomField<TrafficState> crf,
		final NavigableMap<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double> > > lr_model,
		final CRFStructure crf_structure )
	{
		final List<ConditionalRandomField.Clique<TrafficState>> cliques
			= new ArrayList<ConditionalRandomField.Clique<TrafficState>>( crf.Ncliques );
		
		final TrafficFeatureFilter filter = null;
		// Vertex cliques
		for( final Map.Entry<Pair<Integer, Integer>, NavigableMap<Integer, ArrayList<Double> > > e : 
				lr_model.entrySet() ) {
			final int v = positionToVertex( e.getKey() );
			cliques.add( new TrafficVertexClique( 
					makeVertexLRFeatures( e.getKey(), v, e.getValue(), crf_structure ), 
												  filter, v, e.getKey() ) );
		}
		
		// Copy the edge cliques without modification
		for( final ConditionalRandomField.Clique<TrafficState> c : crf.cliques() ) {
			if( c instanceof TrafficEdgeClique ) {
				cliques.add( c.update( Arrays.copyOf( c.parameters(), c.parameters().length ) ) );
			}
		}
		
		assert( cliques.size() == crf.cliques().length );
		
		final ConditionalRandomField.Clique<TrafficState>[] array_type = new ConditionalRandomField.Clique[0];
		final ConditionalRandomField<TrafficState> updated = new ConditionalRandomField<TrafficState>( 
			crf.cardinalities(), crf.adjacency_list(), 
			(ConditionalRandomField.Clique<TrafficState>[]) cliques.toArray( array_type ) );
		
		return updated;
	}
	
	private void initializeState() {
		_state.init(_n != null ? _n._hmObjects : null, _i._hmObjects,  
				_d._hmTypes, _d._hmPVariables, _d._hmCPF,
				_i._alInitState, _n == null ? null : _n._alNonFluents, 
				_d._alStateConstraints, _d._exprReward, _i._nNonDefActions);
	}
	
	private TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> collectActionVars() 
			throws EvalException {
			
			TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>> action_vars = 
				new TreeMap<PVAR_NAME,ArrayList<ArrayList<LCONST>>>();
		
			for (PVAR_NAME p : _state._alActionNames) {
				ArrayList<ArrayList<LCONST>> gfluents = _state.generateAtoms(p);
				action_vars.put(p, gfluents);
			}
			
			return action_vars;
	}
	
	public static void main(String[] args) {
		new CRFStructure("./traffic_mdp.rddl", "./traffic_mdp_5_5.rddl");
	}
	
}
